#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import math
import time
import random
from geometry_msgs.msg import PoseStamped, TwistStamped, Point, Quaternion
from sensor_msgs.msg import NavSatFix
from builtin_interfaces.msg import Time
from rclpy.time import Time as RclpyTime

class BoatSimulationNode(Node):
    def __init__(self):
        super().__init__('boat_simulation_node')
        
        # 创建发布者
        self.pose_pub = self.create_publisher(PoseStamped, '/mavros/local_position/pose', 10)
        self.velocity_pub = self.create_publisher(TwistStamped, '/mavros/local_position/velocity_body', 10)
        self.global_pos_pub = self.create_publisher(NavSatFix, '/mavros/global_position/global', 10)
        
        # 起点经纬度
        self.start_lat = 32.115566
        self.start_lon = 119.360629
        
        # 圆形运动参数
        self.circle_radius = 30.0  # 半径30米
        self.circle_duration = 40.0  # 40秒完成一个圆
        self.angular_velocity = 2 * math.pi / self.circle_duration  # 角速度 rad/s
        self.linear_speed = self.circle_radius * self.angular_velocity  # 线速度 m/s
        
        # 抖动参数
        self.noise_amplitude_pos = 1.5  # 位置抖动幅度 (米)
        self.noise_amplitude_heading = 0.1  # 朝向抖动幅度 (弧度，约5.7度)
        self.noise_frequency = 0.3  # 抖动频率因子
        
        # 地球半径 (米)
        self.earth_radius = 6378137.0
        
        # 时间控制
        self.start_time = None
        self.current_phase = 'counterclockwise'  # 'counterclockwise' 或 'clockwise'
        
        # 噪声种子，确保平滑的抖动
        self.noise_seed = 0
        
        # 定时器，20Hz频率发布数据
        self.timer = self.create_timer(0.05, self.publish_data)
        
        self.get_logger().info('Boat simulation node started')
        self.get_logger().info(f'Starting position: lat={self.start_lat:.6f}, lon={self.start_lon:.6f}')
        self.get_logger().info(f'Circle radius: {self.circle_radius}m, Duration: {self.circle_duration}s each')
        self.get_logger().info(f'Base speed: {self.linear_speed:.2f}m/s with ±0.3m/s variation')
        self.get_logger().info(f'Initial heading: East (90°)')
        self.get_logger().info(f'Noise amplitude: pos={self.noise_amplitude_pos}m, heading={self.noise_amplitude_heading:.2f}rad')
        
    def generate_smooth_noise(self, t, frequency, amplitude, seed_offset=0):
        """生成平滑的噪声"""
        # 使用多个频率的正弦波叠加生成更自然的噪声
        noise = 0
        noise += math.sin(t * frequency + seed_offset) * 0.5
        noise += math.sin(t * frequency * 1.7 + seed_offset + 1.2) * 0.3
        noise += math.sin(t * frequency * 2.3 + seed_offset + 2.4) * 0.2
        return noise * amplitude
        
    def meters_to_lat_lon(self, dx, dy, ref_lat, ref_lon):
        """将米制位移转换为经纬度偏移"""
        # 纬度偏移 (北为正)
        dlat = dy / self.earth_radius * (180.0 / math.pi)
        
        # 经度偏移 (东为正)，考虑纬度影响
        dlon = dx / (self.earth_radius * math.cos(ref_lat * math.pi / 180.0)) * (180.0 / math.pi)
        
        return dlat, dlon
    
    def euler_to_quaternion(self, roll, pitch, yaw):
        """欧拉角转四元数"""
        qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
        qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
        qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
        qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
        
        return qx, qy, qz, qw
    
    def publish_data(self):
        current_time = self.get_clock().now()
        
        if self.start_time is None:
            self.start_time = current_time
            
        # 计算elapsed time (秒)
        elapsed_seconds = (current_time - self.start_time).nanoseconds / 1e9
        
        # 确定当前阶段和角度
        if elapsed_seconds < self.circle_duration:
            # 逆时针圆 (0-40秒)
            self.current_phase = 'counterclockwise'
            # 修改：起始朝向正东，所以角度从-π/2开始 (正东对应-π/2)
            angle = self.angular_velocity * elapsed_seconds - math.pi/2
            phase_time = elapsed_seconds
        elif elapsed_seconds < 2 * self.circle_duration:
            # 顺时针圆 (40-80秒)
            self.current_phase = 'clockwise'
            phase_time = elapsed_seconds - self.circle_duration
            # 修改：起始朝向正东，顺时针运动
            angle = -self.angular_velocity * phase_time - math.pi/2
        else:
            # 重新开始
            self.start_time = current_time
            angle = -math.pi/2  # 重新开始时朝向正东
            phase_time = 0
            self.current_phase = 'counterclockwise'
        
        # 生成位置噪声
        noise_x = self.generate_smooth_noise(elapsed_seconds, self.noise_frequency, self.noise_amplitude_pos, 0)
        noise_y = self.generate_smooth_noise(elapsed_seconds, self.noise_frequency, self.noise_amplitude_pos, 3.14)
        
        # 计算圆形轨迹上的位置 (以起点为圆心) + 抖动
        x = self.circle_radius * math.cos(angle) + noise_x
        y = self.circle_radius * math.sin(angle) + noise_y
        
        # 生成朝向噪声
        heading_noise = self.generate_smooth_noise(elapsed_seconds, self.noise_frequency * 1.5, self.noise_amplitude_heading, 1.57)
        
        # 计算航向角 (切线方向) + 抖动
        if self.current_phase == 'counterclockwise':
            yaw = angle + math.pi/2 + heading_noise  # 逆时针：切线方向
        else:
            yaw = angle - math.pi/2 + heading_noise  # 顺时针：切线方向
        
        # 归一化yaw到[-pi, pi]
        while yaw > math.pi:
            yaw -= 2 * math.pi
        while yaw < -math.pi:
            yaw += 2 * math.pi
        
        # 发布本地位置和姿态 (/mavros/local_position/pose)
        pose_msg = PoseStamped()
        pose_msg.header.stamp = current_time.to_msg()
        pose_msg.header.frame_id = 'map'
        
        pose_msg.pose.position.x = x
        pose_msg.pose.position.y = y
        pose_msg.pose.position.z = 0.0
        
        # 转换为四元数
        qx, qy, qz, qw = self.euler_to_quaternion(0, 0, yaw)
        pose_msg.pose.orientation.x = qx
        pose_msg.pose.orientation.y = qy
        pose_msg.pose.orientation.z = qz
        pose_msg.pose.orientation.w = qw
        
        self.pose_pub.publish(pose_msg)
        
        # 计算实际速度（考虑抖动的影响）
        # 使用更低的频率生成缓慢的速度变动，幅度±0.3m/s
        speed_variation = self.generate_smooth_noise(elapsed_seconds, self.noise_frequency * 0.5, 0.3, 2.1)
        actual_speed = self.linear_speed + speed_variation
        
        # 发布速度 (/mavros/local_position/velocity_body)
        velocity_msg = TwistStamped()
        velocity_msg.header.stamp = current_time.to_msg()
        velocity_msg.header.frame_id = 'base_link'
        
        # 前进速度 (body frame x轴) + 速度抖动
        velocity_msg.twist.linear.x = actual_speed
        velocity_msg.twist.linear.y = 0.0
        velocity_msg.twist.linear.z = 0.0
        
        # 角速度 (绕z轴) + 角速度抖动
        angular_noise = self.generate_smooth_noise(elapsed_seconds, self.noise_frequency * 1.2, 0.02, 4.2)
        if self.current_phase == 'counterclockwise':
            velocity_msg.twist.angular.z = self.angular_velocity + angular_noise
        else:
            velocity_msg.twist.angular.z = -self.angular_velocity + angular_noise
            
        self.velocity_pub.publish(velocity_msg)
        
        # 发布全球位置 (/mavros/global_position/global)
        # 将局部坐标转换为经纬度
        dlat, dlon = self.meters_to_lat_lon(x, y, self.start_lat, self.start_lon)
        
        global_msg = NavSatFix()
        global_msg.header.stamp = current_time.to_msg()
        global_msg.header.frame_id = 'map'
        
        global_msg.latitude = self.start_lat + dlat
        global_msg.longitude = self.start_lon + dlon
        global_msg.altitude = 0.0
        
        # 设置状态和协方差
        global_msg.status.status = 0  # STATUS_FIX
        global_msg.status.service = 1  # SERVICE_GPS
        global_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
        
        self.global_pos_pub.publish(global_msg)
        
        # 每5秒打印一次状态
        if int(elapsed_seconds) % 5 == 0 and int(elapsed_seconds * 20) % 100 == 0:
            yaw_deg = yaw * 180.0 / math.pi
            # 修改：航向计算 (0°为北，90°为东)
            course = 90 - yaw_deg
            if course < 0:
                course = 360 + course
                
            self.get_logger().info(
                f'Phase: {self.current_phase}, Time: {phase_time:.1f}s, '
                f'Pos: ({x:.1f}, {y:.1f})m, Course: {course:.1f}°, '
                f'Speed: {actual_speed:.1f}m/s, '
                f'GPS: ({global_msg.latitude:.6f}, {global_msg.longitude:.6f})'
            )

def main(args=None):
    rclpy.init(args=args)
    
    node = BoatSimulationNode()
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('Simulation stopped by user')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()